// testm.cpp : Defines the entry point for the console application.
//

#include "stdafx.h"

#include <time.h>
#include <math.h>

void sleep(unsigned int mseconds)
{
    clock_t goal = mseconds + clock();
    while (goal > clock());
}

int _tmain(int argc, _TCHAR* argv[])
{
	std::cout << "Pod motion test...\n";
	std::cout << "-Initialising data structures...\n";
	TPlatform platform;
	TFilterGroup filters;
	TPositionParameters positions;
	TStateMachineState state;
	BYTE stateChanged;
	float x,y,z,roll,pitch,yaw;
	int i=1;
	int j=0;
	int revolutions=0;
	float p=0;
	float radius = 0.5;
	x=0;
	y=0;
	z=0;
	roll=0;
	pitch=0;
	yaw=0;

	std::cout << "--Reading platform parameter file\n";

	if (readPlatformParameterFile("platform.pp",0,&platform)!=ppfSUCCESS)
	{
		std::cerr << "\n*** ERROR: Problem reading platform.pp file ***\n";
	}

	std::cout << "--Reading filter parameter file\n";
	
    if (readFilterParameterFile("host.fil",0,&filters)!=fpfSUCCESS)
	{
		std::cerr << "\n*** ERROR: Problem reading host.fil file ***\n";
	}

	std::cout << "--Reading position parameter file\n";
    if (readPositionParameterFile("dof.pos",0,&positions)!=fpfSUCCESS)
	{
		std::cerr << "\n*** ERROR: Problem reading dof.pos file ***\n";
	}

	std::cout << "-Initialising state machine on motion control card...\n";
	
	if (initialiseStateMachine(0x208,"main.out",&platform,platform.mdu.stateMachinePeriod)!=erSUCCESS)
	{
		std::cerr << "\n*** ERROR: Problem initialising state machine on MCC ***\n";
	}
	
		std::cout << "Configuring filters...\n";
		for (int counter=0;counter<cnNUMBER_OF_FILTER_BLOCKS;counter++)
		{
			if (setFilterBlock(counter,&filters.filterBlock[counter],platform.mdu.stateMachinePeriod)!=erSUCCESS)
			{
				std::cerr << "\n*** ERROR: Problem with setFilterBlock ***\n";
			}
		}
		if (setMotionCueing(filters.motionCueingOn,filters.specificForcesOn,filters.adaptionOn)!=erSUCCESS) 
		{	
			std::cerr << "\n*** ERROR: Problem with setMotionCueing ***\n";
		}

	std::cout << "-Entering serviceStateMachine loop..\n";
	while (1)
	{
		if (serviceStateMachine(&state, &stateChanged)) {
		switch (state) 
		{
		case smsUNKNOWN:
			std::cout << "--State: smsUNKNOWN.\n";
			break;
		case smsINITIAL:
			std::cout << "--State: smsINITIAL.\n";
			break;
		case smsWAITING_FOR_LINK_OFF:
			std::cout << "--State: smsWAITING_FOR_LINK_OFF.\n";
			break;
		case smsPSU_DISABLED:
			std::cout << "--State: smsPSU_DISABLED. Selecting kinematics, setting parked & neutral positions and enabling PSU.\n";
		if (setKinematicsType(ktSTEWART_PLATFORM)!=erSUCCESS)
		{
			std::cerr << "\n*** ERROR: Problem setting kinematics ***\n";
			break;
		}
        if (setParkedPosition(positions.parkedPosition)!=erSUCCESS)
		{		
			std::cerr << "\n*** ERROR: Problem setting parked position ***\n";
			break;
		}

        if (setNeutralPosition(positions.neutralPosition)!=erSUCCESS)
		{
			std::cerr << "\n*** ERROR: Problem setting neutral position ***\n";
			break;
		}
		setPlatformCommand(pcENABLE_PSU);
		break;
		case smsWAITING_FOR_AMPLIFIER_TEMP_HEALTHY:
		std::cout << "--State: smsWAITING_FOR_AMPLIFIER_TEMP_HEALTHY.\n";
			break;
		case smsWAITING_FOR_MOTOR_TEMP_HEALTHY:
		std::cout << "--State: smsWAITING_FOR_MOTOR_TEMP_HEALTHY.\n";
			break;
		case smsWAITING_FOR_INTERLOCKS_UNHEALTHY:
		std::cout << "--State: smsWAITING_FOR_INTERLOCKS_UNHEALTHY.\n";
			if (j==2)
				j=0;
			break;
		case smsWAITING_FOR_START:
		std::cout << "--State: smsWAITING_FOR_START.\n";
			break;
		case smsWAITING_FOR_END_OF_INRUSH:
		std::cout << "--State: smsWAITING_FOR_END_OF_INRUSH.\n";
			break;
		case smsWAITING_FOR_LINK_IN_SPEC:
		std::cout << "--State: smsWAITING_FOR_LINK_IN_SPEC.\n";
			break;
		case smsWAITING_FOR_DUMP_FUSE_HEALTHY:
		std::cout << "--State: smsWAITING_FOR_DUMP_FUSE_HEALTHY.\n";
			break;
		case smsWAITING_FOR_AMPLIFIERS_HEALTHY:
		std::cout << "--State: smsWAITING_FOR_AMPLIFIERS_HEALTHY.\n";
			break;
		case smsHOMING:
		std::cout << "--State: smsHOMING.\n";
			break;
		case smsGOING_TO_NEUTRAL:
		std::cout << "--State: smsGOING_TO_NEUTRAL.\n";
			break;
		case smsIDLE:
		std::cout << "--State: smsIDLE.\n";
		setHostDataLatches(x,y,z,roll,pitch,yaw,-1);
		setPlatformCommand(pcFOLLOW);
			break;
		case smsGOING_TO_PARKED:
		std::cout << "--State: smsGOING_TO_PARKED.\n";
			break;
		case smsSTOPPING:
		std::cout << "--State: smsSTOPPING.\n";
			break;
		case smsFOLLOWING:
		if (j==0)
			j++;
		setHostDataLatches(x,y,z,roll,pitch,yaw,-1);
		std::cout << "--State: smsFOLLOWING.\n";
        break;
			break;
		case smsINITIATING_EMERGENCY_STOP:
		std::cout << "--State: smsINITIATING_EMERGENCY_STOP.\n";
			break;
		case smsUPS_REMOVING_DC_LINK:
		std::cout << "--State: smsUPS_REMOVING_DC_LINK.\n";
			break;
		case smsUPS_WAITING_FOR_LINK_OFF:
		std::cout << "--State: smsUPS_WAITING_FOR_LINK_OFF.\n";
			break;
		case smsUPS_WAITING_FOR_PLATFORM_STOPPED:
		std::cout << "--State: smsUPS_WAITING_FOR_PLATFORM_STOPPED.\n";
			break;
		case smsUPS_WAITING_FOR_END_OF_INRUSH:
		std::cout << "--State: smsUPS_WAITING_FOR_END_OF_INRUSH.\n";
			break;
		case smsUPS_WAITING_FOR_LINK_IN_SPEC:
		std::cout << "--State: smsUPS_WAITING_FOR_LINK_IN_SPEC.\n";
			break;
		case smsUPS_WAITING_FOR_DUMP_FUSE_HEALTHY:
		std::cout << "--State: smsUPS_WAITING_FOR_DUMP_FUSE_HEALTHY.\n";
			break;
		case smsUPS_RECOVERY:
		std::cout << "--State: smsUPS_RECOVERY.\n";
			break;
		case smsUPS_WAITING_FOR_AMPLIFIER_HEALTHY:
		std::cout << "--State: smsUPS_WAITING_FOR_AMPLIFIER_HEALTHY.\n";
			break;
		case smsUPS_PARKING:
		std::cout << "--State: smsUPS_PARKING.\n";
			break;
		case smsLAST:
		std::cout << "--State: smsLAST.\n";
			break;
		}
	}
	
	
	
	if(j==1)
	{
		// go to the initial point on the circle
		if(x>=0.49)
		{
			j++;
		}
		else
		{
			x += 0.01f;
		}
	}
	else if (j==2)
	{
		// circular motion
		if (p>=6.28)
		{
			p=0;
			revolutions++;
			if (revolutions==5)
			{
				setPlatformCommand(pcGO_TO_PARKED);
				setPlatformCommand(pcSTOP);
			}
		}
		x = radius * cos(p);
		y = radius * sin(p);

		std::cout << "x: ";
		std::cout << x;
		std::cout << "\ny: ";
		std::cout << y;
		std::cout << "\n";
		std::cout << "P: ";
		std::cout << p;
		std::cout << "\n";
		p += 0.04f;
	}
	sleep(15);
}
return 0;
}

 

 